﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace SmartMathLibrary
{
    /// <summary>
    /// This class provides the approximation of the hessian matrix by using extrapolation.
    /// </summary>
    [Serializable]
    public class RealHessianExtrapolationApproximator
    {
        /// <summary>
        /// Internal array for extrapolation.
        /// </summary>
        private readonly double[] STANDARD_H = {1e-1, 1e-2, 1e-3, 1e-4, 1e-5};

        /// <summary>
        /// Internal array for extrapolation.
        /// </summary>
        private readonly double[] STANDARD_H_SQUARE = {1e-2, 1e-4, 1e-6, 1e-8, 1e-10};

        /// <summary>
        /// The source function for the to approximate partial derivative.
        /// </summary>
        private IHardMultivariateRealFunction sourceFunction;

        /// <summary>
        /// Initializes a new instance of the <see cref="RealHessianExtrapolationApproximator"/> class.
        /// </summary>
        /// <param name="sourceFunction">The source function for the to approximate partial derivative.</param>
        public RealHessianExtrapolationApproximator(HardMultivariateRealFunction sourceFunction)
        {
            if (sourceFunction == (HardMultivariateRealFunction) null)
            {
                throw new ArgumentNullException("sourceFunction");
            }

            if (sourceFunction.HardRealFunctionPointer == (HardMultivariateRealFunctionPointer) null)
            {
                throw new ArgumentNullException("sourceFunction");
            }

            this.sourceFunction = sourceFunction;
        }

        /// <summary>
        /// Initializes a new instance of the <see cref="RealHessianExtrapolationApproximator"/> class.
        /// </summary>
        /// <param name="sourceFunction">The source function for the to approximate partial derivative.</param>
        public RealHessianExtrapolationApproximator(IHardMultivariateRealFunction sourceFunction)
        {
            if (sourceFunction == (HardMultivariateRealFunction) null)
            {
                throw new ArgumentNullException("sourceFunction");
            }

            this.sourceFunction = sourceFunction;
        }

        /// <summary>
        /// Gets or sets the source function for the to approximate partial derivative.
        /// </summary>
        /// <value>The source function for the to approximate partial derivative.</value>
        public IHardMultivariateRealFunction SourceFunction
        {
            get { return sourceFunction; }
            set { sourceFunction = value; }
        }

        /// <summary>
        /// Approximates the specified real function derivatives by using extrapolation and returns the jacobian matrix.
        /// </summary>
        /// <param name="x">The vector x at which the function derivative should be calculate.</param>
        /// <param name="continuous">Set true if the function is continuous. This will speed up the computation.</param>
        /// <returns>
        /// The specified slope at the positions of x.
        /// </returns>
        public Matrix Approximate(GeneralVector x, bool continuous)
        {
            int m = x.Count - 1;
            double[] f1h = new double[5];
            double[,] result = new double[x.Count,x.Count];
            GeneralVector vec = new GeneralVector(x.Count);

            for (int k = 0; k < x.Count; k++)
            {
                vec[k] = x[k];
            }

            for (int i = 0; i < x.Count; i++)
            {
                if (continuous)
                {
                    m = i;
                }

                for (int j = 0; j <= m; j++)
                {
                    for (int k = 0; k < 5; k++)
                    {
                        double vecPlusH;
                        double vecMinusH;

                        if (i != j)
                        {
                            vec[i] = x[i] + STANDARD_H[k];
                            vec[j] = x[j] + STANDARD_H[k];
                            vecPlusH = this.sourceFunction.SolveAt(vec);

                            vec[i] = x[i] - STANDARD_H[k];
                            vec[j] = x[j] - STANDARD_H[k];
                            vecMinusH = this.sourceFunction.SolveAt(vec);

                            vec[i] = x[i] + STANDARD_H[k];
                            vec[j] = x[j] - STANDARD_H[k];

                            double vecPlusHMinusK = this.sourceFunction.SolveAt(vec);

                            vec[i] = x[i] - STANDARD_H[k];
                            vec[j] = x[j] + STANDARD_H[k];

                            double vecMinusHPlusK = this.sourceFunction.SolveAt(vec);

                            f1h[k] = (vecPlusH - vecPlusHMinusK - vecMinusHPlusK + vecMinusH)/(4*STANDARD_H_SQUARE[k]);
                        }
                        else
                        {
                            vec[j] = x[j] + STANDARD_H[k];
                            vecPlusH = this.sourceFunction.SolveAt(vec);

                            vec[j] = x[j] - STANDARD_H[k];
                            vecMinusH = this.sourceFunction.SolveAt(vec);

                            vec[j] = x[j];
                            double vecStdH = this.sourceFunction.SolveAt(vec);

                            f1h[k] = (vecPlusH - 2*vecStdH + vecMinusH)/STANDARD_H_SQUARE[k];
                        }
                    }

                    double interpolatedData = NevillePolynomialInterpolation.NevilleInterpolation(5,
                                                                                                  this.STANDARD_H_SQUARE,
                                                                                                  f1h);

                    result[i, j] = interpolatedData;

                    if (continuous)
                    {
                        result[j, i] = interpolatedData;
                    }
                }
            }

            return new Matrix(result);
        }
    }
}